/*
 * rx11_functions.c
 * Created on July 8, 2005; 4:22 PM
 * by VRamadurai
 */
#include <stdio.h>
#include <stdlib.h>

/************ Private Header Files **************/
#include "HsTypes.h"
#include "PhyDebug.h"
#include "PhyStruct.h"
#include "PmdGlobals.h"
#include "PhyConstants.h"
#include "rx_functions.h"
#include "rx1_functions.h"
#include "rx55_functions.h"
#include "rx11_functions.h"
#include "iqdma_3500.h"
#include "equalizer.h"
#include "thread_globals.h"
#include "threads_common.h"//add 627

inline void
derotate_11_1(
				hs_int16_t cos_val,
				hs_int16_t sin_val,
				hs_int16_t *sincos_t,
				hs_int16_t *outBuffIQ,
				hs_int16_t *currPtrI,
				hs_int16_t *currPtrQ,
				hs_int16_t *iq_scale,
			    hs_int16_t *iq_bias
			)
{
	hs_int16_t ssOutIQ[32];
	hs_int16_t sincos_fixed[32];
	hs_int16_t tempI[32];
    hs_int16_t tempQ[32];
    hs_int16_t ssInI[32];
    hs_int16_t ssInQ[32];

    __sb_rbroad(sincos_fixed, (cos_val<<16), 0);
    __sb_rbroad(sincos_fixed+16, (sin_val<<16), 0);

    __sb_rshf1(tempI, tempI+16, currPtrI, currPtrI+16, 7);
	__sb_rshf1(tempQ, tempQ+16, currPtrQ, currPtrQ+16, 7);

    __sb_rshra(tempI, tempI, iq_scale);
    __sb_rsubf(tempI, iq_bias, tempI);
    __sb_rshra(tempQ, tempQ, iq_scale);
    __sb_rsubf(tempQ, iq_bias, tempQ);

    __sb_rmuls(ssInI, tempI, sincos_t);
    __sb_rmacs(ssInI, tempQ, sincos_t+32, ssInI);
    __sb_rmuls(ssInQ, tempI, sincos_t+32);
    __sb_rmsubs(ssInQ, tempQ, sincos_t, ssInQ);

    __sb_rmuls(tempI, ssInI, sincos_fixed);
    __sb_rmacs(ssOutIQ, ssInQ, sincos_fixed+16, tempI);
    __sb_rmuls(tempQ, ssInI, sincos_fixed+16);
    __sb_rmsubs(ssOutIQ+16, ssInQ, sincos_fixed, tempQ);

    __sb_rshf(outBuffIQ, outBuffIQ+16, ssOutIQ, ssOutIQ+16, 7);
}

inline void
derotate_11_2(
				hs_int16_t cos_val,
				hs_int16_t sin_val,
				hs_int16_t *sincos_t,
				hs_int16_t *outBuffIQ,
				hs_int16_t *currPtrI,
				hs_int16_t *currPtrQ,
				hs_int32_t offset,
				hs_int16_t *piStartPtrI,
			    hs_int16_t *piStartPtrQ,
			    hs_int16_t *iq_scale,
		        hs_int16_t *iq_bias
			)
{
    //hs_int32_t i;

    hs_int16_t temp[16];
    hs_int16_t sincos_fixed[32];
	hs_int16_t tempI[32];
    hs_int16_t tempQ[32];
    hs_int16_t ssInI[32+32];
    hs_int16_t ssInQ[32+32];
    hs_int16_t ssOutIQ[32];

    __sb_rbroad(sincos_fixed, (cos_val<<16), 0);
    __sb_rbroad(sincos_fixed+16, (sin_val<<16), 0);

    __sb_rshra(temp, currPtrI, iq_scale);
    __sb_rsubf(ssInI, iq_bias, temp);
    __sb_rshra(temp, currPtrQ, iq_scale);
	__sb_rsubf(ssInQ, iq_bias, temp);

    __sb_rshra(temp, currPtrI+16, iq_scale);
	__sb_rsubf(ssInI+16, iq_bias, temp);
	__sb_rshra(temp, currPtrQ+16, iq_scale);
	__sb_rsubf(ssInQ+16, iq_bias, temp);

    __sb_rshra(temp, piStartPtrI, iq_scale);
	__sb_rsubf(ssInI+(32-offset), iq_bias, temp);
	__sb_rshra(temp, piStartPtrI+16, iq_scale);
	__sb_rsubf(ssInI+(32-offset)+16, iq_bias, temp);
	__sb_rshf1(tempI, tempI+16, ssInI, ssInI+16, 7);

    __sb_rshra(temp, piStartPtrQ, iq_scale);
    __sb_rsubf(ssInQ+(32-offset), iq_bias, temp);
    __sb_rshra(temp, piStartPtrQ+16, iq_scale);
    __sb_rsubf(ssInQ+(32-offset)+16, iq_bias, temp);
    __sb_rshf1(tempQ, tempQ+16, ssInQ, ssInQ+16, 7);

    __sb_rmuls(ssInI, tempI, sincos_t);
	__sb_rmuls(ssInQ, tempI, sincos_t+32);
    __sb_rmacs(ssInI, tempQ, sincos_t+32, ssInI);
    __sb_rmsubs(ssInQ, tempQ, sincos_t, ssInQ);

    __sb_rmuls(tempI, ssInI, sincos_fixed);
    __sb_rmacs(ssOutIQ, ssInQ, sincos_fixed+16, tempI);
    __sb_rmuls(tempQ, ssInI, sincos_fixed+16);
    __sb_rmsubs(ssOutIQ+16, ssInQ, sincos_fixed, tempQ);

    __sb_rshf(outBuffIQ, outBuffIQ+16, ssOutIQ, ssOutIQ+16, 7);

}

static inline
void sync_on_dma(
                    hs_int32_t  siInitAddrI,
                    hs_int32_t  blocks
                )
{
    //hs_int32_t i;
    hs_int32_t gap;

    hs_uint32_t base = ICR_ADDRESS_CORE_CHANNEL(RX_CORE, MMIO_SB3500A2D_A_OFF); //Core 1, PSDA

    while((((gap=(ICR_GET(base,REG_A2DISTR))-(siInitAddrI & 0x0fffff))< blocks && gap>=0 )  ||
        (gap<0 && (GLOBAL_BUF_SIZE_RX*sizeof(hs_int16_t)+gap) < blocks))
    )
    {
            __sb_barrier();
    }
}

void
wlan_do_11mbps(
                struct HS_Phy11bStr  *HS_Phy11bPtrTh,
                ePHYSTATES_TH *ePhyState,
                hs_int32_t NSymbols,
                hs_int16_t *ssSineCosineTable
             )
{
    hs_int32_t offset;
    //hs_int32_t i;

    hs_int16_t *eqOutIQBase;
    hs_int16_t *eqOutI;
    hs_int16_t *iq_scale;
    hs_int16_t *iq_bias;
    hs_int16_t *piStartPtrI;
    hs_int16_t *piStartPtrQ;

    hs_int16_t *currPtrI;
    hs_int16_t *currPtrQ;
    hs_int16_t *sincos_t;

    hs_int32_t eqoutbuff_toggle;
    hs_uint32_t uiRotorTableIndex;
    hs_int16_t cos_val;
    hs_int16_t sin_val;
    hs_int32_t symbol_count;
    hs_int32_t siRotorTableCounter;
    hs_int16_t *piEndPtrI;

    hs_int32_t siPhaseUpdate = HS_Phy11bPtrTh->siPhaseUpdate;
    volatile hs_int32_t *thread1to2_flag1_ptr = &thread1to2_flag1;

    symbol_count = 0;
    sincos_t = HS_Phy11bPtrTh->sincos_t;

    piEndPtrI = HS_Phy11bPtrTh->piEndPtrI;
    siRotorTableCounter = HS_Phy11bPtrTh->siRotorTableCounter;

    currPtrI = HS_Phy11bPtrTh->piRdPtrI;
    currPtrQ = HS_Phy11bPtrTh->piRdPtrQ;

    iq_scale = HS_Phy11bPtrTh->iq_scale;
    iq_bias = HS_Phy11bPtrTh->iq_bias;
    piStartPtrI = HS_Phy11bPtrTh->piStartPtrI;
    piStartPtrQ = HS_Phy11bPtrTh->piStartPtrQ;
    eqOutIQBase = eqOutIQ_shared_11;

    eqoutbuff_toggle = HS_Phy11bPtrTh->eqoutbuff_toggle_th2;

    while(thread3toall_rate_flag == 0){
		__sb_barrier();
    }

    if(*HS_Phy11bPtrTh->rx_status_l == HS_HEAD_CRC_ERROR){
		*ePhyState = PHY_STATE_SWITCH_OFF_TH;
		return;
	}

    while(1)
    {
        sync_on_dma((hs_int32_t)currPtrI, 64);

        START_CLOCK_11

        offset=(hs_int32_t)(currPtrI + 32 - piEndPtrI);

        eqOutI = eqOutIQBase + EQOUTBUFFSIZE*2*eqoutbuff_toggle;
        eqoutbuff_toggle ^= 1;

        uiRotorTableIndex =(((siRotorTableCounter+ 0x200000)&0x7fffffff) >> 22);
		cos_val = ssSineCosineTable[uiRotorTableIndex]; //VR
		sin_val = ssSineCosineTable[513+uiRotorTableIndex];

        if(offset < 0){
        	derotate_11_1(cos_val, sin_val, sincos_t,
						  &buff_LEri[2*(N_PRE+N_POST)], currPtrI, currPtrQ, iq_scale, iq_bias);

        	currPtrI += 32;
            currPtrQ += 32;
        }
        else{
        	derotate_11_2(cos_val, sin_val, sincos_t,
						  &buff_LEri[2*(N_PRE+N_POST)], currPtrI, currPtrQ, offset,
        				  piStartPtrI, piStartPtrQ, iq_scale, iq_bias);

        	currPtrI = piStartPtrI + offset;
        	currPtrQ = piStartPtrQ + offset;
        }

        AdaptiveEqData_16_asm(buff_LEri, w_LEri, eqOutI);

        /** sync with thread2 **/
        while(*thread1to2_flag1_ptr == 1){
            __sb_barrier();
        }

        *thread1to2_flag1_ptr = 1;

        siRotorTableCounter += (2*siPhaseUpdate*16);
        symbol_count+=2;

        if(symbol_count >= NSymbols){
            *ePhyState = PHY_STATE_SWITCH_OFF_TH;
            break;
        }
    }

    PRINT_CYCLES_11("data 11mbps 1");

    HS_Phy11bPtrTh->piRdPtrI = currPtrI;
    HS_Phy11bPtrTh->piRdPtrQ = currPtrQ ;
}
